//
// Created by benxb on 2022/3/6.
//


#include <ctime>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <timer.h>
#include <common/ini_reader.h>
#include <fstream>
#include <kitti_reader.h>
#include <or_reader.h>
#include <gtest/gtest.h>
#include <ns_reader.h>
#include <status_thread.h>
#include <toml.h>
#include <filesystem>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <sqlite3.h>
#include <catelog/column.h>
#include <access/database.h>
#include <semantic_kitti_reader.h>

std::string table_name = "picsad";
std::shared_ptr<benxdb::Database> db;

void Init() {
    // create table
    std::vector<benxdb::Column> columns;
    columns.emplace_back("sweep", benxdb::INTEGER);
    columns.emplace_back("timestamp", benxdb::BIGINT);
    columns.emplace_back("x", benxdb::FLOAT, benxdb::LINEAR_INDEX);
    columns.emplace_back("y", benxdb::FLOAT);
    columns.emplace_back("z", benxdb::FLOAT);
    columns.emplace_back("classification", benxdb::INTEGER);
    db = std::make_shared<benxdb::Database>();
    db->CreateTable(std::make_shared<benxdb::Table>(table_name, columns));
}


void WritePoints(SemanticKittiReader* data_reader, const std::string& data_dir) {
    if (data_reader == nullptr) {
        return;
    }
    // cpu and mem monitor
    benchmark::StatusThread statusThread(benchmark::GetCurrentPid(), "picsad", data_dir);
    std::thread stats_t(statusThread);

    // timer
    benchmark::Timer timer;
    timer.Start();
    Init();
    timer.StopAndRecord("Init time.");

    // open data file
    std::vector<float> x_arr;
    std::vector<float> y_arr;
    std::vector<float> z_arr;
    std::vector<uint16_t> ins_arr;
    std::vector<uint16_t> cls_arr;

    int sweep = 0;
    while(data_reader->HasMoreSweep()) {
        sweep++;
        if (sweep % 10 == 1) {
            std::cout << sweep << std::endl;
        }
        // read data
        data_reader->ReadMapFile(x_arr, y_arr, z_arr, ins_arr, cls_arr);
        long long timestamp = data_reader->ReadTimestamp();

        timer.Start();
        benxdb::PointCloud<benxdb::PointSTXYZC> point_cloud(x_arr.size());
        for (int i = 0; i < x_arr.size(); i++) {
            point_cloud[i].sweep = sweep;
            point_cloud[i].timestamp = timestamp;
            point_cloud[i].x = x_arr[i];
            point_cloud[i].y = y_arr[i];
            point_cloud[i].z = z_arr[i];
            point_cloud[i].classification = cls_arr[i];
        }
        db->AppendPointCloud(table_name, point_cloud);
        timer.Pause();
        x_arr.clear();
        y_arr.clear();
        z_arr.clear();
    }

    timer.StopAndRecord("Load time.");

    timer.Start();

    timer.StopAndRecord("Close time");
    timer.PrintAll();
    timer.SaveRecords(data_dir + "picsad_time.txt");
    benchmark::g_status_mtx.lock();
    stats_t.join();
    benchmark::g_status_mtx.unlock();
}

int main() {
    toml::table conf;
    try {
        conf = toml::parse_file("/home/benoxo/picsad/benchmark/config.toml");
    } catch (const toml::parse_error& err) {
        std::cerr << "Parsing failed:\n" << err << "\n";
        return 1;
    }

    auto test_cases = conf["test_cases"].as_array();
    for (int i = 0; i < test_cases->size(); i++) {
        std::string case_name = test_cases->get(0)->value_or("");
        table_name = case_name;
        auto tcase = conf[case_name];
        auto reader_name = tcase["reader"].value_or("");
        auto data_dir = tcase["data_dir"].value_or("");
        auto *reader = new SemanticKittiReader(data_dir);
        WritePoints(reader, data_dir);
        delete reader;
    }
}